//
// Created by nsy on 2020/11/20.
//

_Pragma("once")
#include "../sensor_info/camera_info.h"
#include "shader.h"

#include <pcl/point_types.h>
#include <pcl/search/octree.h>
#include <pcl/common/transforms.h>

class CommonShader : public Shader{
public:
    typedef pcl::PointXYZ Point;
    typedef pcl::PointCloud<Point> PointCloud;
    typedef PointCloud::Ptr PointCloudPtr;
    typedef PointCloud::ConstPtr PointCloudConstPtr;
    typedef pcl::octree::OctreePointCloudSearch<Point> Octree;
    typedef Octree::Ptr OctreePtr;

    typedef std::vector<CameraInfo, Eigen::aligned_allocator<CameraInfo>> CameraList;
    typedef pcl::PointXYZRGB PointRGB;
    typedef pcl::PointCloud<PointRGB> PointCloudRGB;

    /**
     * 设置待处理点云
     * @param[input] cloud
     * @return
     */
    virtual void setInputCloud(PointCloudPtr cloud);

    /**
     * 设置八叉树体素大小
     * @param[input] resolution
     * @return
     */
    virtual void setResolution(float resolution);

    /**
     * 获取八叉树体素大小
     * @return
     */
    virtual float getResolution();

    /**
     * 根据相机位姿信息来进行点云空间变换
     * @param[input] cloud_in 待变换点云
     * @param[output] cloud_out 变换后点云
     * @param[input] camera 对应相机
     * @return
     */
    void transformCloudByCamera(PointCloud cloud_in, PointCloud &cloud_out,
                                      CameraInfo camera);

    /**
     * 检测空间点到摄像机之间有无被其他点遮挡
     *   需要先将点云变换到相机坐标系下
     * @param[input] pt 输入点
     * @param[input] octree 点云对应的八叉树
     * @return 是否被阻挡
     */
    bool isPointOccluded(const Point &p, OctreePtr octree);


    /**
     * 根据空间点和相机信息获取平面坐标(x,y)
     *   需要先将点云变换到相机坐标系下
     * 图像左上为(0,0),横轴为x，纵轴为y
     * 0 <= x < 图像宽度  0 <= y < 图像高度 为整数
     * @param[input] p
     * @param[input] camera
     * @param[output] XY_coordinates 平面坐标，不在视角内时为(-1,-1)
     * @return
     */
    void getXYCoordinates(const Point &p, CameraInfo camera,
                                Eigen::Vector2i &XY_coordinates);

    /**
     * 根据空间点和相机信息获取平面坐标(x,y)以及点到相机距离
     *   需要先将点云变换到相机坐标系下
     * 图像左上为(0,0),横轴为x，纵轴为y
     * 0 <= x < 图像宽度  0 <= y < 图像高度 为整数
     * 为减少计算，距离采用曼哈顿距离并非欧氏距离
     * @param[input] p
     * @param[input] camera
     * @param[output] XY_coordinates 平面坐标，不在视角内时为(-1,-1)
     * @param[output] distance 点到相机距离，不在视角内时为MAX_DOUBLE
     * @return
     */
    void getXYCoordinates(const Point &p, CameraInfo camera,
                                Eigen::Vector2i &XY_coordinates, float &distance);


protected:

    /** 待处理的点云 */
    PointCloudPtr cloud_;

    /** 八叉树体素大小，用于射线检测，建议与体素滤波体素大小相同 */
    float resolution_;
};